﻿using ELFSharp.ELF;
using HidApi;
using Microsoft.VisualBasic;
using MySoftLab.DapFlashCore.Probes.DapAccess;
using MySoftLab.DapFlashCore.Utils;
using System;
using System.Collections.Generic;
using System.ComponentModel.Design;
using System.Diagnostics;
using System.Drawing;
using System.Formats.Asn1;
using System.Linq;
using System.Net;
using System.Runtime.Intrinsics.Arm;
using System.Runtime.Intrinsics.X86;
using System.Text;
using System.Threading.Tasks;
using System.Xml.Linq;
using static MySoftLab.DapFlashCore.Probes.DapAccess.DAPAccessIntf;
using static System.Runtime.InteropServices.JavaScript.JSType;

namespace MySoftLab.DapFlashCore.CoreSight
{
    public abstract class AccessPort
    {
        protected readonly DebugPort _dp;
        protected APAddressBase _address;
        protected readonly DapAccessCmsisDap _link;
        protected uint _romAddr = 0;
        private object? _romTable = null;
        private bool _initedPrimary = false;
        private bool _initedSecondary = false;
        private uint? _idr;
        protected uint _flags;
        protected APVersion _apVersion;
        protected HashSet<uint> _transferSizes = new HashSet<uint>() { 32 };

        public static uint AP_ROM_TABLE_ADDR_REG = 0xf8;
        public static uint AP_ROM_TABLE_FORMAT_MASK = 0x2;
        public static uint AP_ROM_TABLE_ENTRY_PRESENT_MASK = 0x1;

        public static Dictionary<uint, uint> MEM_AP_IDR_TO_WRAP_SIZE = new Dictionary<uint, uint>()
        {
            { 0x24770011, 0x1000 },   // Used on m4 & m3 - Documented in arm_cortexm4_processor_trm_100166_0001_00_en.pdf
                                      //                   and arm_cortexm3_processor_trm_100165_0201_00_en.pdf
            { 0x44770001, 0x400 },    // Used on m1 - Documented in DDI0413D_cortexm1_r1p0_trm.pdf
            { 0x04770031, 0x400 },    // Used on m0+? at least on KL25Z, KL46, LPC812
            { 0x04770021, 0x400 },    // Used on m0? used on nrf51, lpc11u24
            { 0x64770001, 0x400 },    // Used on m7
            { 0x74770001, 0x400 }    // Used on m0+ on KL28Z
         };

        public bool HasRomTable { get; set; } = false;

        // AP Control and Status Word definitions
        public const uint CSW_SIZE = 0x00000007;
        public const uint CSW_SIZE8 = 0x00000000;
        public const uint CSW_SIZE16 = 0x00000001;
        public const uint CSW_SIZE32 = 0x00000002;
        public const uint CSW_ADDRINC = 0x00000030;
        public const uint CSW_NADDRINC = 0x00000000;
        public const uint CSW_SADDRINC = 0x00000010;
        public const uint CSW_PADDRINC = 0x00000020;
        public const uint CSW_DBGSTAT = 0x00000040;
        public const uint CSW_TINPROG = 0x00000080;
        public const uint CSW_HPROT = 0x02000000;
        public const uint CSW_MSTRTYPE = 0x20000000;
        public const uint CSW_MSTRCORE = 0x00000000;
        public const uint CSW_MSTRDBG = 0x20000000;
        public const uint CSW_RESERVED = 0x01000000;

        public const uint DEFAULT_CSW_VALUE = CSW_SADDRINC;

        // @brief Mask for register address within the AP address space.
        //
        // v1 APs have a 256 byte register space. v2 APs have a 4 kB register space. This mask is
        // larger than the APv1 register space, but this is not problematic because v1 APs only have
        // the 8-bit APSEL in bits 31:24 of the address, thus no possibility of conflict.
        public const uint APREG_MASK = 0x00000ffc;

        // MEM-AP register addresses
        public const uint MEM_AP_CSW = 0x00;
        public const uint MEM_AP_TAR = 0x04;
        public const uint MEM_AP_DRW = 0x0C;
        public const uint MEM_AP_TRR = 0x24; // Only APv2 with ERRv1
        public const uint MEM_AP_BASE_HI = 0xF0;
        public const uint MEM_AP_CFG = 0xF4;
        public const uint MEM_AP_BASE = 0xF8;

        public const uint MEM_AP_CFG_TARINC_MASK = 0x000f0000;
        public const int MEM_AP_CFG_TARINC_SHIFT = 16;
        public const uint MEM_AP_CFG_ERR_MASK = 0x00000f00;
        public const int MEM_AP_CFG_ERR_SHIFT = 8;
        public const uint MEM_AP_CFG_DARSIZE_MASK = 0x000000f0;
        public const int MEM_AP_CFG_DARSIZE_SHIFT = 4;
        public const uint MEM_AP_CFG_LD_MASK = 0x00000004;
        public const uint MEM_AP_CFG_LA_MASK = 0x00000002;

        // AP Control and Status Word definitions
        public const uint CSW_SIZE64 = 0x00000003;
        public const uint CSW_SIZE128 = 0x00000004;
        public const uint CSW_SIZE256 = 0x00000005;
        public const uint CSW_DEVICEEN = 0x00000040;
        public const uint CSW_ERRNPASS = 0x00010000; // # MEM-APv2 only
        public const uint CSW_ERRSTOP = 0x00020000;  // # MEM-APv2 only
        public const uint CSW_SDEVICEEN = 0x00800000; // # Also called SPIDEN in ADIv5
        public const uint CSW_DBGSWEN = 0x80000000; // # Only present in CSSoC-400 APB-AP, RES0 in others

        // # AP BASE register masks
        public const uint AP_BASE_FORMAT_MASK = 0x2;
        public const uint AP_BASE_ENTRY_PRESENT_MASK = 0x1;
        public const uint AP_BASE_BASEADDR_MASK = 0xfffffffc;
        public const uint AP_BASE_LEGACY_NOTPRESENT = 0xffffffff; // # Legacy not present value
        public const uint AP_BASE_LEGACY_BASEADDR_MASK = 0xfffff000;

        // ## Offset of IDR register in an APv1.
        public const uint AP_IDR = 0xFC;
        // ## Offset of IDR register in an APv2.
        public const uint APv2_IDR = 0xDFC;

        public const int APSEL_SHIFT = 24;

        public const uint AP_IDR_REVISION_MASK = 0xf0000000;
        public const int AP_IDR_REVISION_SHIFT = 28;
        public const uint AP_IDR_JEP106_MASK = 0x0ffe0000;
        public const int AP_IDR_JEP106_SHIFT = 17;
        public const uint AP_IDR_CLASS_MASK = 0x0001e000;
        public const int AP_IDR_CLASS_SHIFT = 13;
        public const uint AP_IDR_VARIANT_MASK = 0x000000f0;
        public const int AP_IDR_VARIANT_SHIFT = 4;
        public const uint AP_IDR_TYPE_MASK = 0x0000000f;

        public const uint MEM_AP_CFG_ERR_V1 = 1;
        public const uint MEM_AP_TRR_ERR_MASK = 0x00000001;

        public const uint CSW_HPROT_MASK = 0x0f000000; // # HPROT[3:0]
        public const int CSW_HPROT_SHIFT = 24;

        public const uint CSW_HNONSEC_MASK = 0x40000000;
        public const int CSW_HNONSEC_SHIFT = 30;

        // # Debug Exception and Monitor Control Register
        public const uint DEMCR = 0xE000EDFC;
        // # DWTENA in armv6 architecture reference manual
        public const uint DEMCR_TRCENA = 1 << 24;

        //// @brief Arm JEP106 code
        //
        // - [6:0] = 0x3B, Arm's JEP106 identification code
        // - [12:7] = 4, the number of JEP106 continuation codes for Arm
        public const uint AP_JEP106_ARM = 0x23b;

        // @brief Arm China JEP106 code
        //
        // - [6:0] = 0x75, JEP106 identification code
        // - [12:7] = 10, number of JEP106 continuation codes
        public const uint AP_JEP106_ARM_CHINA = 0x575;

        // AP classes
        public const uint AP_CLASS_JTAG_AP = 0x0;
        public const uint AP_CLASS_COM_AP = 0x1; // SDC-600 (Chaucer)
        public const uint AP_CLASS_MEM_AP = 0x8; // AHB-AP, APB-AP, AXI-AP

        // # MEM-AP type constants
        public const uint AP_TYPE_AHB = 0x1;
        public const uint AP_TYPE_APB = 0x2;
        public const uint AP_TYPE_AXI = 0x4;
        public const uint AP_TYPE_AHB5 = 0x5;
        public const uint AP_TYPE_APB4 = 0x6;
        public const uint AP_TYPE_AXI5 = 0x7;
        public const uint AP_TYPE_AHB5_HPROT = 0x8;

        // # AP flags.
        public const uint AP_4K_WRAP = 0x1; // # The AP has a 4 kB auto-increment modulus.
        public const uint AP_ALL_TX_SZ = 0x2; // # The AP is known to support 8-, 16-, and 32-bit transfers, *unless* Large Data is implemented.
        public const uint AP_MSTRTYPE = 0x4; // # The AP is known to support the MSTRTYPE field.
        public const uint AP_DBGSWEN = 0x8; // # The AP is known to support the DBGSWEN flag.

        //  HNONSECURE bits;
        public const uint SECURE = 0;
        public const uint NONSECURE = 1;

        //  HPROT bits;
        public const uint HPROT_DATA = 0x01;
        public const uint HPROT_INSTR = 0x00;
        public const uint HPROT_PRIVILEGED = 0x02;
        public const uint HPROT_USER = 0x00;
        public const uint HPROT_BUFFERABLE = 0x04;
        public const uint HPROT_NONBUFFERABLE = 0x00;
        public const uint HPROT_CACHEABLE = 0x08;
        public const uint HPROT_NONCACHEABLE = 0x00;
        public const uint HPROT_LOOKUP = 0x10;
        public const uint HPROT_NO_LOOKUP = 0x00;
        public const uint HPROT_ALLOCATE = 0x20;
        public const uint HPROT_NO_ALLOCATE = 0x00;
        public const uint HPROT_SHAREABLE = 0x40;
        public const uint HPROT_NONSHAREABLE = 0x00;

        public static Dictionary<uint, uint> TRANSFER_SIZE = new()
        {
            { 8, CSW_SIZE8 }, { 16, CSW_SIZE16 }, { 32, CSW_SIZE32 },
        };

        //// Map from AP IDR fields to AccessPort subclass.
        //
        // The dict maps from a 4-tuple of (JEP106 code, AP class, variant, type) to 3-tuple (name, class, flags).
        //
        // Known AP IDRs:
        // 0x24770011 AHB-AP with 0x1000 wrap and MSTRTYPE
        //               Used on m4 & m3 - Documented in arm_cortexm4_processor_trm_100166_0001_00_en.pdf
        //               and arm_cortexm3_processor_trm_100165_0201_00_en.pdf
        // 0x34770001 AHB-AP Documented in DDI0314H_coresight_components_trm.pdf
        // 0x44770001 AHB-AP Used on m1 - Documented in DDI0413D_cortexm1_r1p0_trm.pdf
        // 0x04770031 AHB-AP Used on m0+? at least on KL25Z, KL46, LPC812
        // 0x04770021 AHB-AP Used on m0? used on nrf51, lpc11u24
        // 0x04770041 AHB-AP Used on m7, RT1050
        // 0x64770001 AHB-AP Used on m7, documented in DDI0480G_coresight_soc_trm.pdf
        // 0x74770001 AHB-AP Used on m0+ on KL28Z
        // 0x84770001 AHB-AP Used on K32W042
        // 0x14770005 AHB5-AP Used on M33. Note that M33 r0p0 incorrect fails to report this IDR.
        // 0x04770025 AHB5-AP Used on M23.
        // 0x54770002 APB-AP used on STM32H743, from CSSoC-400
        // 0x34770017 AXI5-AP from Corstone-700
        public static Dictionary<(uint, uint, uint, uint), (string, Type, uint)> AP_TYPE_MAP = new Dictionary<(uint, uint, uint, uint), (string, Type, uint)>() {
            //   |JEP106        |Class              |Var|Type                    |Name      |Class
            { (AP_JEP106_ARM, AP_CLASS_JTAG_AP,   0,  0),                     ("JTAG-AP", typeof(AccessPort), 0   ) },
            { (AP_JEP106_ARM, AP_CLASS_COM_AP,    0,  0),                     ("SDC-600", typeof(AccessPort), 0   ) },
            { (AP_JEP106_ARM, AP_CLASS_MEM_AP,    0,  AP_TYPE_AHB),           ("AHB-AP",  typeof(AHB_AP),     AP_ALL_TX_SZ ) },
            { (AP_JEP106_ARM, AP_CLASS_MEM_AP,    1,  AP_TYPE_AHB),           ("AHB-AP",  typeof(AHB_AP),     AP_ALL_TX_SZ|AP_4K_WRAP|AP_MSTRTYPE ) },
            { (AP_JEP106_ARM, AP_CLASS_MEM_AP,    2,  AP_TYPE_AHB),           ("AHB-AP",  typeof(AHB_AP),     AP_ALL_TX_SZ ) },
            { (AP_JEP106_ARM, AP_CLASS_MEM_AP,    3,  AP_TYPE_AHB),           ("AHB-AP",  typeof(AHB_AP),     AP_ALL_TX_SZ ) },
            { (AP_JEP106_ARM, AP_CLASS_MEM_AP,    4,  AP_TYPE_AHB),           ("AHB-AP",  typeof(AHB_AP),     AP_ALL_TX_SZ ) },
            { (AP_JEP106_ARM, AP_CLASS_MEM_AP,    0,  AP_TYPE_APB),           ("APB-AP",  typeof(MEM_AP),     AP_DBGSWEN   ) },
            { (AP_JEP106_ARM, AP_CLASS_MEM_AP,    0,  AP_TYPE_AXI),           ("AXI-AP",  typeof(MEM_AP),     AP_ALL_TX_SZ ) },
            { (AP_JEP106_ARM, AP_CLASS_MEM_AP,    0,  AP_TYPE_AHB5),          ("AHB5-AP", typeof(AHB_AP),     AP_ALL_TX_SZ ) },
            { (AP_JEP106_ARM, AP_CLASS_MEM_AP,    1,  AP_TYPE_AHB5),          ("AHB5-AP", typeof(AHB_AP),     AP_ALL_TX_SZ ) },
            { (AP_JEP106_ARM, AP_CLASS_MEM_AP,    2,  AP_TYPE_AHB5),          ("AHB5-AP", typeof(AHB_AP),     AP_ALL_TX_SZ ) },
            { (AP_JEP106_ARM, AP_CLASS_MEM_AP,    0,  AP_TYPE_APB4),          ("APB4-AP", typeof(MEM_AP),     0   ) },
            { (AP_JEP106_ARM, AP_CLASS_MEM_AP,    0,  AP_TYPE_AXI5),          ("AXI5-AP", typeof(MEM_AP),     AP_ALL_TX_SZ ) },
            { (AP_JEP106_ARM, AP_CLASS_MEM_AP,    1,  AP_TYPE_AXI5),          ("AXI5-AP", typeof(MEM_AP),     AP_ALL_TX_SZ ) },
            { (AP_JEP106_ARM, AP_CLASS_MEM_AP,    0,  AP_TYPE_AHB5_HPROT),    ("AHB5-AP", typeof(MEM_AP),     AP_ALL_TX_SZ ) },
            { (AP_JEP106_ARM_CHINA,
                    AP_CLASS_MEM_AP,    1,  AP_TYPE_AHB5),          ("AHB5-AP", typeof(AHB_AP),     AP_ALL_TX_SZ ) }
    };

        private uint _variant;
        private uint _revision;
        private uint _apClass;
        private uint _apType;

        public AccessPort(DebugPort dp, APAddressBase apAddress, uint? idr = null, string? name = null, uint flags = 0, object? cmpid = null)
        {
            _dp = dp;
            _address = apAddress;
            _link = dp.Link;
            _idr = idr;
            _flags = flags;
            _apVersion = apAddress.APVersion;
        }

        public virtual void Init()
        {
            //if(!_initedPrimary)
            //{
            //    _idr = ReadReg(DebugPort.AP_REG["IDR"]);

            //    // Init ROM table
            //    _romAddr = ReadReg(AP_ROM_TABLE_ADDR_REG);
            //    _hasRomTable = (_romAddr != 0xffffffff) && ((_romAddr & AP_ROM_TABLE_ENTRY_PRESENT_MASK) != 0);
            //    _romAddr &= 0xfffffffc; // clear format and present bits;
            //    _initedPrimary = true;
            //}


            // Read IDR if it wasn't given to us in the ctor.
            if (null == _idr)
            {
                _idr = ReadReg(_address.IdrAddress).Value;
            }

            _variant = (_idr.Value & AP_IDR_VARIANT_MASK) >> AP_IDR_VARIANT_SHIFT;
            _revision = (_idr.Value & AP_IDR_REVISION_MASK) >> AP_IDR_REVISION_SHIFT;

            // # Get the type name for this AP.
            _apClass = (_idr.Value & AP_IDR_CLASS_MASK) >> AP_IDR_CLASS_SHIFT;
            _apType = _idr.Value & AP_IDR_TYPE_MASK;
        }

        /// <summary>
        /// Determine if an AP exists with the given AP number. Only applicable for ADIv5.
        /// </summary>
        /// <param name="dp">DebugPort instance</param>
        /// <param name="apNum">The AP number (APSEL) to probe</param>
        /// <returns>indicating if a valid AP exists with APSEL=ap_num.</returns>
        public static bool Probe(DebugPort dp, uint apNum)
        {
            var idr = dp.ReadAP(apNum << APSEL_SHIFT | AP_IDR).Value;
            return idr != 0;
        }

        /// <summary>
        /// Create a new AP object.
        /// Determines the type of the AP by examining the IDR value and creates a new
        /// AP object of the appropriate class. See #AP_TYPE_MAP for the mapping of IDR
        /// fields to class.
        /// </summary>
        /// <returns></returns>
        public static AccessPort? Create(DebugPort dp, APAddressBase apAddress, object? cmpid = null)
        {
            // Attempt to read the IDR for this APSEL. If we get a zero back then there is
            // no AP present, so we return None.
            // Check AP version and set the offset to the control and status registers.
            var idr = dp.ReadAP(apAddress.Address + apAddress.IdrAddress).Value;
            if (idr == 0)
            {
                throw new Exception($"Invalid AP address ({apAddress})");
            }

            // Extract IDR fields used for lookup.
            var designer = (idr & AP_IDR_JEP106_MASK) >> AP_IDR_JEP106_SHIFT;
            var apClass = (idr & AP_IDR_CLASS_MASK) >> AP_IDR_CLASS_SHIFT;
            var variant = (idr & AP_IDR_VARIANT_MASK) >> AP_IDR_VARIANT_SHIFT;
            var apType = idr & AP_IDR_TYPE_MASK;

            // Get the AccessPort class to instantiate.
            var key = (designer, apClass, variant, apType);

            string name;
            Type klass;
            uint flags;

            try
            {
                (name, klass, flags) = AP_TYPE_MAP[key];
            }
            catch (Exception e)
            {
                // The AP ID doesn't match, but we can recognize unknown MEM-APs.
                if (apClass == AP_CLASS_MEM_AP && designer == AP_JEP106_ARM)
                {
                    name = "MEM-AP";
                    klass = typeof(MEM_AP);
                }
                else
                {
                    name = "";
                    klass = typeof(AccessPort);
                }
                flags = 0;
            }

            AccessPort? ap = null;

            if(klass == typeof(MEM_AP))
            {
                ap = new MEM_AP(dp, apAddress, idr, name, flags, cmpid);
            }
            else if(klass == typeof(AHB_AP))
            {
                ap = new AHB_AP(dp, apAddress, idr, name, flags, cmpid);
            } 

            if (ap != null)
            {
                ap.Init();
            }
            return ap;
        }

        public abstract DelayResult<uint> ReadReg(uint addr, bool now = true);

        public virtual void WriteReg(uint addr, uint data)
        {
            _dp.WriteAP(_address.Address + addr, data);
        }

        public abstract void WriteMemory(uint addr, uint data, uint transferSize = 32);

        public abstract DelayResult<uint> Read32(uint addr, bool now = true);

        public abstract DelayResult<uint> ReadMemory(uint addr, uint transferSize = 32, bool now = true);

        public abstract void WriteMemoryBlock32(uint addr, uint[] data);

        public abstract void FindComponents();

        public abstract uint[] ReadMemoryBlock32(uint addr, int size);

        public abstract void WriteBlockMemoryUnaligned8(uint addr, byte[] data);
    }

    public class MEM_AP : AccessPort
    {
        private uint _autoIncrementPageSize = 0x400;
        private ulong _addressMask = 0xffffffff;
        private uint _regOffset = 0;
        protected uint _csw = 0;
        private uint _cachedCsw;
        private bool _hasLargeData;
        private uint _darCount;
        private uint _implHprot;
        private uint _implHnonsec;
        private uint _hprot;
        private uint _hnonsec;

        public MEM_AP(DebugPort dp, APAddressBase apAddress, uint? idr = null, string? name = null, uint flags = 0, object? cmpid = null) : base(dp, apAddress, idr, name, flags, cmpid)
        {
            // Default to the smallest size supported by all targets.
            // A size smaller than the supported size will decrease performance
            // due to the extra address writes, but will not create any
            // read/write errors.
            _autoIncrementPageSize = 0x400;
            // TODO

            // ## Default HPROT value for CSW.
            _hprot = HPROT_DATA | HPROT_PRIVILEGED;

            // ## Default HNONSEC value for CSW.
            _hnonsec = SECURE;

            _csw = DEFAULT_CSW_VALUE;

            // # Certain MEM-APs support a DBGSWEN control in the AP's CSW register. When set to zero, software running
            // # on the device is prevented from accessing the memory space downstream from the MEM-AP. This feature is
            // # deprecated in ADIv6, and Arm recommends to never clear the bit when implemented.
            if ((_flags & AP_DBGSWEN) != 0)
            {
                _csw |= CSW_DBGSWEN;
            }

            _cachedCsw = 0xffffffff;

            // ## Auto-increment wrap modulus.
            // #
            // # The AP_4K_WRAP flag indicates a 4 kB wrap size. Otherwise it defaults to the smallest
            // # size supported by all targets. A size smaller than the supported size will decrease
            // # performance due to the extra address writes, but will not create any read/write errors.
            _autoIncrementPageSize = (_flags & AP_4K_WRAP) != 0 ? 0x1000 : (uint)0x400;

            // ## Number of DAR registers.
            _darCount = 0;
        }

        private void InitCfg()
        {
            // Read MEM-AP CFG register.
            var cfg = ReadReg(_regOffset + MEM_AP_CFG).Value;

            // Check for 64-bit address support.
            if ((cfg & MEM_AP_CFG_LA_MASK) != 0)
            {
                _addressMask = 0xffffffffffffffff;
            }

            // # Check for Large Data extension.
            if ((cfg & MEM_AP_CFG_LD_MASK) != 0)
            {
                _hasLargeData = true;
            }

            // # Check v2 MEM-AP CFG fields.
            if (_apVersion == APVersion.APv2)
            {
                // # Set autoinc page size if TARINC is non-zero. Otherwise we've already set the
                // # default of 1 kB in the ctor.
                var tarinc = (cfg & MEM_AP_CFG_TARINC_MASK) >> MEM_AP_CFG_TARINC_SHIFT;
                if (tarinc != 0)
                {
                    _autoIncrementPageSize = (uint)(1 << (int)(9 + tarinc));
                }

                // # Determine supported err mode.
                var err = (cfg & MEM_AP_CFG_ERR_MASK) >> MEM_AP_CFG_ERR_SHIFT;
                if (err == MEM_AP_CFG_ERR_V1)
                {
                    // # Configure the error mode such that errors are passed upstream, but they don't
                    // # prevent future transactions.
                    _csw &= ~(CSW_ERRSTOP | CSW_ERRNPASS);

                    // # Clear TRR in case we attach to a device with a sticky error already set.
                    WriteReg(_regOffset + MEM_AP_TRR, MEM_AP_TRR_ERR_MASK);
                }


                // # Init size of DAR register window.
                var darsize = (cfg & MEM_AP_CFG_DARSIZE_MASK) >> MEM_AP_CFG_DARSIZE_SHIFT;
                _darCount = (uint)((1 << (int)darsize) / 4);
            }

        }

        /// <summary>
        /// Read ROM table base address
        /// </summary>
        private void InitRomTableBase()
        {
            var baseAddr = ReadReg(_regOffset + MEM_AP_BASE).Value;
            var isAdiv5Base = (baseAddr & AP_BASE_FORMAT_MASK) != 0;
            var isBasePresent = (baseAddr & AP_BASE_ENTRY_PRESENT_MASK) != 0;
            var isLegacyBasePresent = !isAdiv5Base && !isBasePresent;
            if (isLegacyBasePresent)
            {
                HasRomTable = true;
                _romAddr = baseAddr & AP_BASE_LEGACY_BASEADDR_MASK; // # clear format and present bits
            }
            else if (baseAddr == AP_BASE_LEGACY_NOTPRESENT || !isBasePresent)
            {
                HasRomTable = false;
                _romAddr = 0;
            }
            else if (isAdiv5Base && isBasePresent)
            {
                HasRomTable = true;
                _romAddr = baseAddr & AP_BASE_BASEADDR_MASK; // # clear format and present bits
            }
            else
            {
                throw new Exception($"invalid AP BASE value 0x{baseAddr:X8}");
            }
        }

        /// <summary>
        /// Utility to verify whether the MEM-AP supports a given transfer size.
        /// From ADIv6:
        /// If the CSW.Size field is written with a value corresponding to a size that is not supported,
        /// or with a reserved value: A read of the field returns a value corresponding to a supported
        ///        size.
        /// </summary>
        /// <param name="sz"></param>
        /// <returns></returns>
        private Func<bool> TestTransferSize(uint sz, uint originalCsw)
        {
            // # Write CSW_SIZE to select requested transfer size.
            WriteReg(_regOffset + MEM_AP_CSW, originalCsw & ~CSW_SIZE | sz);

            // # Read back CSW and see if SIZE matches what we wrote.
            var cswCb = ReadReg(_regOffset + MEM_AP_CSW, false);

            return () => (cswCb.Action() & CSW_SIZE) == sz;
        }

        /// <summary>
        /// Determine supported transfer sizes.
        /// If the #AP_ALL_TX_SZ flag is set, then we know a priori that this AP implementation
        /// supports 8-, 16- and 32- transfer sizes.If the Large Data extension is implemented, then this
        /// flag is ignored.
        /// Note in ADIv6: "If a MEM-AP implementation does not support the Large Data Extension, but does
        /// support various access sizes, it must support word, halfword, and byte accesses."
        /// So, if the Large Data extension is present, then we have to individually test each
        /// transfer size (aside from the required 32-bit).
        /// If Large Data is not present, then only one non-32-bit transfer size needs to be tested to
        /// determine if the AP supports both 8- and 16-bit transfers in addition to the required 32-bit.
        /// </summary>
        private void InitTranferSizes(uint originalCsw)
        {
            // # If AP_ALL_TX_SZ is set, we can skip the test. Double check this by ensuring that LD is not enabled.
            if ((_flags & AP_ALL_TX_SZ) != 0 && !_hasLargeData)
            {
                _transferSizes.Clear();
                _transferSizes.Add(8);
                _transferSizes.Add(16);
                _transferSizes.Add(32);
                return;
            }

            // Thus if LD ext is not present, we only need to test one size.
            if (_hasLargeData)
            {
                // Need to scan all sizes except 32-bit, which is required.
                uint[] SIZES_TO_TEST = { CSW_SIZE8, CSW_SIZE16, CSW_SIZE64, CSW_SIZE128, CSW_SIZE256 };

                _transferSizes.Clear();
                _transferSizes.Add(32);
                foreach (var sz in SIZES_TO_TEST)
                {
                    if (TestTransferSize(sz, originalCsw).Invoke())
                    {
                        _transferSizes.Add((uint)(8 * (1 << (int)sz)));
                    }
                }
            }
            else if (TestTransferSize(CSW_SIZE16, originalCsw).Invoke())
            {
                _transferSizes.Clear();
                _transferSizes.Add(8);
                _transferSizes.Add(16);
                _transferSizes.Add(32);
            }
        }

        /// <summary>
        /// Setter for current HPROT value used for memory transactions.
        /// The bits of HPROT have the following meaning.Not all bits are implemented in all
        /// MEM-APs.AHB-Lite only implements HPROT[3:0].
        /// HPROT[0] = 1 data access, 0 instr fetch<br/>
        /// HPROT[1] = 1 priviledge, 0 user<br/>
        /// HPROT[2] = 1 bufferable, 0 non bufferable<br/>
        /// HPROT[3] = 1 cacheable/modifable, 0 non cacheable<br/>
        /// HPROT[4] = 1 lookup in cache, 0 no cache<br/>
        /// HPROT[5] = 1 allocate in cache, 0 no allocate in cache<br/>
        /// HPROT[6] = 1 shareable, 0 non shareable<br/>
        /// </summary>
        public uint Hprot
        {
            set
            {
                _hprot = value & CSW_HPROT_MASK >> CSW_HPROT_SHIFT;
                _csw = _csw & ~CSW_HPROT_MASK | _hprot << CSW_HPROT_SHIFT;
            }
        }

        /// <summary>
        /// Setter for current HNONSEC value used for memory transactions.
        /// Not all MEM-APs support control of HNONSEC.In particular, only the AHB5-AP used for
        /// v8-M Cortex-M systems does.The AXI-AP for Cortex-A systems also allows this control.
        ///  @param value 0 is secure, 1 is non-secure.
        /// </summary>
        public uint Hnonsec
        {
            set
            {
                _hnonsec = value & CSW_HNONSEC_MASK >> CSW_HNONSEC_SHIFT;
                _csw = _csw & ~CSW_HNONSEC_MASK | _hnonsec << CSW_HNONSEC_SHIFT;
            }
        }

        /// <summary>
        /// Init HPROT HNONSEC.
        /// Determines the implemented bits of HPROT and HNONSEC in this MEM-AP. The defaults for these
        /// fields of the CSW are based on the implemented bits.
        /// </summary>
        private void InitHprot(uint originalCsw)
        {
            var defaultHprot = (originalCsw & CSW_HPROT_MASK) >> CSW_HPROT_SHIFT;
            var defaultHnonsec = (originalCsw & CSW_HNONSEC_MASK) >> CSW_HNONSEC_SHIFT;
            Console.WriteLine($"default HPROT={defaultHprot:X8} HNONSEC={defaultHnonsec:X8}");

            // # Now attempt to see which HPROT and HNONSEC bits are implemented.
            WriteReg(_regOffset + MEM_AP_CSW, originalCsw | CSW_HNONSEC_MASK | CSW_HPROT_MASK);
            var csw = ReadReg(_regOffset + MEM_AP_CSW).Value;

            _implHprot = (csw & CSW_HPROT_MASK) >> CSW_HPROT_SHIFT;
            _implHnonsec = (csw & CSW_HNONSEC_MASK) >> CSW_HNONSEC_SHIFT;
            Console.WriteLine($"implemented HPROT={_implHprot:X8} HNONSEC={_implHnonsec:X8}");

            // # Update current HPROT and HNONSEC, and the current base CSW value.
            Hprot = _hprot & _implHprot;
            Hnonsec = _hnonsec & _implHnonsec;
        }

        public override void Init()
        {
            base.Init();

            //# Read initial CSW. Superclass register access methods are used to avoid the CSW cache.
            var originalCsw = ReadReg(_regOffset + MEM_AP_CSW).Value;

            // Run the init tests.
            InitCfg();
            InitTranferSizes(originalCsw);
            InitHprot(originalCsw);
            InitRomTableBase();

            // # Restore unmodified value of CSW.
            WriteReg(_regOffset + MEM_AP_CSW, originalCsw);
        }

        /// <summary>
        /// Security states for a processor with the Security extension
        /// </summary>
        private enum SecurityState
        {
            // PE is in the Non-secure state.
            NONSECURE = 0,
            // ## PE is in the Secure state.
            SECURE = 1
        }

        /// <summary>
        /// Checks whether memory transfers are allowed by this AP for the given security state
        /// Memory transfers may be disabled by an input signal to the AP. This is often done when debug security
        /// is enabled on the device, to disallow debugger access to internal memory.
        /// </summary>
        /// <param name="securityState">security_state One of the @ref pyocd.core.target.Target.SecurityState "SecurityState" enums</param>
        /// <returns>indicating whether memory transfers can be performed in the requested security state. You
        /// may change the security state used for transfers with the hnonsec property and hnonsec_lock() method</returns>
        private bool IsEnabledFor(SecurityState securityState)
        {
            // Call to superclass to read CSW. We want to bypass our CSW cache since the enable signal can change asynchronously.
            var csw = ReadReg(_regOffset + MEM_AP_CSW).Value;

            if (securityState == SecurityState.NONSECURE)
            {
                // Nonsecure transfers are always allowed when security transfers are enabled.
                return (csw & (CSW_DEVICEEN | CSW_SDEVICEEN)) != 0;
            }
            else if (securityState == SecurityState.SECURE)
            {
                return (csw & CSW_SDEVICEEN) != 0;
            }
            else
            {
                throw new Exception("unsupported security state");
            }
        }

        /// <summary>
        /// Whether any memory transfers are allowed by this AP
        /// Memory transfers may be disabled by an input signal to the AP. This is often done when debug security
        /// is enabled on the device, to disallow debugger access to internal memory.
        /// </summary>
        public bool IsEnabled
        {
            get
            {
                return IsEnabledFor(SecurityState.NONSECURE);
            }
        }

        public override void FindComponents()
        {
            // Turn on DEMCR.TRCENA before reading the ROM table. Some ROM table entries can
            // come back as garbage if TRCENA is not set.
            try
            {
                if (HasRomTable)
                {
                    if (!IsEnabled)
                    {
                        Console.WriteLine("Skipping CoreSight discovery for because it is disabled");
                        return;
                    }

                    // TODO
                    //    if not self.is_enabled:
                    //    LOG.warning("Skipping CoreSight discovery for %s because it is disabled", self.short_description)
                    //    return

                    // # Import locally to work around circular import.

                }
            }
            catch (Exception e)
            {

            }
        }

        /// <summary>
        /// Write a single memory location.
        /// </summary>
        /// <param name="addr"></param>
        /// <param name="data"></param>
        /// <param name="transferSize"></param>
        public override void WriteMemory(uint addr, uint data, uint transferSize = 32)
        {
            if ((addr & transferSize / 8 - 1) != 0)
            {
                throw new Exception();
            }

            addr = (uint)(addr & _addressMask);

            if (transferSize != 32)
            {
                throw new ArgumentException();
            }

            var num = _dp.NextAccessNumber;

            WriteReg(DebugPort.AP_REG["CSW"] + _regOffset, _csw | TRANSFER_SIZE[transferSize]);

            if (transferSize == 8)
            {
                data = data << (int)((addr & 0x03) << 3);
            }
            else if (transferSize == 16)
            {
                data = data << (int)((addr & 0x02) << 3);
            }

            try
            {
                WriteReg(DebugPort.AP_REG["TAR"] + _regOffset, addr);
                WriteReg(DebugPort.AP_REG["DRW"] + _regOffset, data);
            }
            catch (Exception ex)
            {
                HandleError(ex, num);
                throw;
            }
        }

        public override void WriteReg(uint addr, uint data)
        {
            var apRegAddr = addr & APREG_MASK;

            // Don't need to write CSW if it's not changing value.
            if (apRegAddr == _regOffset + MEM_AP_CSW)
            {
                if (data == _cachedCsw)
                {
                    var num = _dp.NextAccessNumber;
                    // Console.WriteLine($"write_ap:{num} cached (ap=0x{_address.NominalAddress:X8}; addr=0x{addr:X8}) = 0x{data:X8}");
                    return;
                }
                _cachedCsw = data;
            }

            try
            {
                _dp.WriteAP(_address.Address + addr, data);
            }
            catch (Exception)
            {
                // invalidate cached CSW on exception.
                if (apRegAddr == _regOffset + MEM_AP_CSW)
                {
                    InvalidateCache();
                }
                throw;
            }
        }

        private void InvalidateCache()
        {
            _cachedCsw = 0xffffffff;
        }

        /// <summary>
        /// Read a memory location
        /// By default, a word will be read.
        /// TransferError Raised if the requested transfer size is not supported by the AP.
        /// </summary>
        /// <returns></returns>
        public override DelayResult<uint> ReadMemory(uint addr, uint transferSize = 32, bool now = true)
        {
            if ((addr & transferSize / 8 - 1) != 0)
            {
                throw new ArgumentException();
            }

            addr = (uint)(addr & _addressMask);

            if (!_transferSizes.Contains(transferSize))
            {
                throw new DAPAccessIntf.TransferError($"{transferSize}-bit transfers are not supported");
            }

            var num = _dp.NextAccessNumber;

            DelayResult<uint>? resultCb = null;
            try
            {
                WriteReg(_regOffset + MEM_AP_CSW, _csw | TRANSFER_SIZE[transferSize]);
                WriteReg(_regOffset + MEM_AP_TAR, addr);

                if (transferSize <= 32)
                {
                    resultCb = ReadReg(_regOffset + MEM_AP_DRW, false);
                }
                else
                {
                    // Multi-word transfer.
                    // TODO
                }
            }
            catch (Exception ex)
            {
                HandleError(ex, num);
                throw;
            }

            // 定义回调
            var readMemAction = () =>
            {
                uint res;
                try
                {
                    if (resultCb != null && resultCb.Action != null)
                    {
                        res = resultCb.Action();
                    }
                    else
                    {
                        res = 0;
                    }

                    if (transferSize == 8)
                    {
                        res = res >> (int)((addr & 0x03) << 3) & 0xff;
                    }
                    else if (transferSize == 16)
                    {
                        res = res >> (int)((addr & 0x02) << 3) & 0xffff;
                    }
                }
                catch (Exception ex)
                {
                    HandleError(ex, num);
                    throw;
                }

                return res;
            };

            if (now)
            {
                return new DelayResult<uint>(readMemAction());
            }
            else
            {
                return new DelayResult<uint>(readMemAction);
            }
        }

        /// <summary>
        /// Write a block of aligned words in memory.
        /// </summary>
        /// <param name="addr"></param>
        /// <param name="data"></param>
        /// <exception cref="NotImplementedException"></exception>
        public override void WriteMemoryBlock32(uint addr, uint[] data)
        {
            if ((addr & 0x3) != 0)
            {
                throw new ArgumentException();
            }

            // Console.WriteLine($"------------cur csw={_csw:X8}");

            addr = (uint)(addr & _addressMask);
            var size = data.Length;
            while (size > 0)
            {
                var n = _autoIncrementPageSize - (addr & _autoIncrementPageSize - 1);
                if (size * 4 < n)
                {
                    n = (uint)(size * 4 & 0xfffffffc);
                }

                WriteBlock32Page(addr, data.Take((int)(n / 4)).ToArray());
                data = data.Skip((int)(n / 4)).ToArray();
                size -= (int)(n / 4);
                addr += n;
            }
        }

        /// <summary>
        /// Write a single transaction's worth of aligned words.
        /// The transaction must not cross the MEM-AP's auto-increment boundary.
        /// This method is not locked because it is only called by _write_memory_block32(), which is locked.
        /// </summary>
        /// <param name="addr"></param>
        /// <param name="data"></param>
        private void WriteBlock32Page(uint addr, uint[] data)
        {
            if ((addr & 0x3) != 0)
            {
                throw new ArgumentException();
            }

            var num = _dp.NextAccessNumber;
            // Console.WriteLine($"_write_block32:{num} (ap=0x{_address.NominalAddress:X8}; addr=0x{addr:X8}, size={data.Length}");
            // # put address in TAR
            WriteReg(_regOffset + MEM_AP_CSW, _csw | CSW_SIZE32);
            WriteReg(_regOffset + MEM_AP_TAR, addr);
            try
            {
                _dp.WriteApMultiple(_address.Address + _regOffset + MEM_AP_DRW, data);
            }
            catch (DAPAccessIntf.TransferFaultError error)
            {
                //# Annotate error with target address.
                HandleError(error, num);
                // TODO
                //error.fault_address = addr;
                //error.fault_length = len(data) * 4
                throw;
            }
            catch (Exception e)
            {
                HandleError(e, num);
                throw;
            }

            // Console.WriteLine($"_write_block32:{num}");
        }

        /// <summary>
        /// Read a block of aligned words in memory.
        /// return A list of word values.
        /// </summary>
        public override uint[] ReadMemoryBlock32(uint addr, int size)
        {
            if ((addr & 0x3) != 0)
            {
                throw new ArgumentException();
            }

            addr = (uint)(addr & _addressMask);

            List<uint> resp = [];
            while (size > 0)
            {
                var n = _autoIncrementPageSize - (addr & _autoIncrementPageSize - 1);
                if (size * 4 < n)
                {
                    n = (uint)(size * 4 & 0xfffffffc);
                }

                resp.AddRange(ReadBlock32Page(addr, (int)(n / 4)));

                size -= (int)(n / 4);
                addr += n;
            }

            return resp.ToArray();
        }

        /// <summary>
        /// Read a single transaction's worth of aligned words.
        /// The transaction must not cross the MEM-AP's auto-increment boundary.
        /// This method is not locked because it is only called by _read_memory_block32(), which is locked.
        /// </summary>
        /// <param name="addr"></param>
        /// <param name="size"></param>
        /// <returns></returns>
        /// <exception cref="ArgumentException"></exception>
        private uint[] ReadBlock32Page(uint addr, int size)
        {
            if ((addr & 0x3) != 0)
            {
                throw new ArgumentException();
            }

            var num = _dp.NextAccessNumber;

            // TODO
            // put address in TAR
            WriteReg(_regOffset + MEM_AP_CSW, _csw | CSW_SIZE32);
            WriteReg(_regOffset + MEM_AP_TAR, addr);

            DelayResult<uint[]> resp;
            try
            {
                resp = _dp.ReadApMultiple(_address.Address + _regOffset + MEM_AP_DRW, size);
            }
            catch (DAPAccessIntf.TransferFaultError ex)
            {
                // Annotate error with target address.
                HandleError(ex, num);
                // TODO 
                throw;
            }
            catch (Exception ex)
            {
                HandleError(ex, num);
                throw;
            }

            // Console.WriteLine($"_read_block32:{num}");
            return resp.Value;
        }

        /// <summary>
        /// Shorthand to write a 32-bit word.
        /// </summary>
        /// <param name="addr"></param>
        /// <param name="value"></param>
        public void Write32(uint addr, uint value)
        {
            WriteMemory(addr, value, 32);
        }

        /// <summary>
        /// Shorthand to write a 16-bit halfword.
        /// </summary>
        /// <param name="addr"></param>
        /// <param name="value"></param>
        public void Write16(uint addr, uint value)
        {
            WriteMemory(addr, value, 16);
        }

        /// <summary>
        /// Shorthand to write a byte.
        /// </summary>
        /// <param name="addr"></param>
        /// <param name="value"></param>
        public void Write8(uint addr, uint value)
        {
            WriteMemory(addr, value, 8);
        }

        public override DelayResult<uint> Read32(uint addr, bool now = true)
        {
            return ReadMemory(addr, 32, now);
        }

        public DelayResult<uint> Read16(uint addr, bool now = true)
        {
            return ReadMemory(addr, 16, now);
        }

        public DelayResult<uint> Read8(uint addr, bool now = true)
        {
            return ReadMemory(addr, 8, now);
        }

        public override void WriteBlockMemoryUnaligned8(uint addr, byte[] data)
        {
            var size = data.Length;
            int idx = 0;

            // try to write 8 bits data
            if (size > 0 && (addr & 0x01) != 0)
            {
                WriteMemory(addr, data[idx], 8);
                size -= 1;
                addr += 1;
                idx += 1;
            }

            // try to write 16 bits data
            if (size > 1 && (addr & 0x02) != 0)
            {
                WriteMemory(addr, (uint)(data[idx] | data[idx + 1] << 8), 16);
                size -= 2;
                addr += 2;
                idx += 2;
            }

            // write aligned block of 32 bits
            if (size >= 4)
            {
                var data32 = CommonUtils.ByteListToU32leList(data.Skip(idx).Take(size & ~0x03).ToArray());
                WriteBlockMemoryAligned32(addr, data32);

                int offset = size & ~0x03;
                addr += (uint)offset;
                idx += offset;
                size -= offset;
            }

            // try to write 16 bits data
            if (size > 1)
            {
                WriteMemory(addr, (uint)(data[idx] | data[idx + 1] << 8), 16);
                size -= 2;
                addr += 2;
                idx += 2;
            }

            // try to write 8 bits data
            if (size > 0)
            {
                WriteMemory(addr, data[idx], 8);
                size -= 1;
                addr += 1;
                idx += 1;
            }
        }

        /// <summary>
        /// Write a block of aligned words in memory.
        /// </summary>
        /// <param name="addr"></param>
        /// <param name="data"></param>
        public void WriteBlockMemoryAligned32(uint addr, uint[] data)
        {
            var size = data.Length;

            while (size > 0)
            {
                var n = _autoIncrementPageSize - (addr & _autoIncrementPageSize - 1);
                if (size * 4 < n)
                {
                    n = (uint)(size * 4 & 0xfffffffc);
                }
                WriteMemoryBlock32(addr, data.Take((int)(n / 4)).ToArray());
                data = data.Skip((int)(n / 4)).ToArray();
                size -= (int)(n / 4);
                addr += n;
            }
        }

        ///// <summary>
        ///// Read a block of aligned words in memory
        ///// </summary>
        ///// <param name="addr"></param>
        ///// <param name="size"></param>
        //public uint[] ReadBlockMemoryAligned32(uint addr, int size)
        //{
        //    List<uint> res = new List<uint>();

        //    while(size > 0)
        //    {
        //        var n = (int)(_autoIncrementPageSize - (addr & (_autoIncrementPageSize - 1)));

        //        if(size * 4 < n)
        //        {
        //            n = (int)((size * 4) & 0xfffffffc);
        //        }

        //        var cb = ReadMemoryBlock32(addr, (int)(n / 4));
        //        if(cb.Value != null)
        //        {
        //            res.AddRange(cb.Value);
        //        }

        //        size -= n / 4;
        //        addr += (uint)n;
        //    }

        //    return res.ToArray();
        //}

        public override DelayResult<uint> ReadReg(uint addr, bool now = true)
        {
            return _dp.ReadAP(_address.Address + addr, now);
        }

        private void HandleError(Exception error, int num)
        {
            _dp.HandleError(error, num);
        }
    }
    public class AHB_AP : MEM_AP
    {
        public AHB_AP(DebugPort dp, APAddressBase apAddress, uint? idr = null, string? name = null, uint flags = 0, object? cmpid = null) : base(dp, apAddress, idr, name, flags, cmpid)
        {
        }

        public override void Init()
        {
            base.Init();

            // # Check for and enable the Master Type bit on AHB-APs where it might be implemented.
            if ((_flags & AP_MSTRTYPE) != 0)
            {
                InitMstrtype();
            }
        }

        /// <summary>
        /// Set master type control in CSW.
        /// Only the v1 AHB-AP from Cortex-M3 and Cortex-M4 implements the MSTRTYPE flag to control
        /// whether transactions appear as debugger or internal accesses.
        /// </summary>
        private void InitMstrtype()
        {
            // # Set the master type to "debugger" for AP's that support this field.
            _csw |= CSW_MSTRDBG;
        }

        public override void FindComponents()
        {
            // Turn on DEMCR.TRCENA before reading the ROM table. Some ROM table entries can
            // come back as garbage if TRCENA is not set.
            var demcr = Read32(DEMCR).Value;
            Write32(DEMCR, demcr | DEMCR_TRCENA);
            // Thread.Sleep(100); // TODO important
            // _dp.Flush();

            uint retry = 0;
            while ((Read32(DEMCR).Value & DEMCR_TRCENA) == 0 && retry++ < 10)
            {
                Thread.Sleep(10);
            }

            if (retry >= 10)
            {
                throw new TimeoutException("TRCENA enable failed");
            }

            _dp.Flush();

            base.FindComponents();
        }
    }
}
